/*
  Program Name:  MC_Slave
  Project:       LLH Hopscotch
  Written By:    Daniel Lee
  Revision:      0.7.4
  Change log:   
    0.6.1   Minor fixes
            Fixed a problem where the controller locks on to a channel and never receives a valid signal.
            Fixed an unresolved situation during AUTO mode when the path reached a dead-end.
            Fixed a problem that occurs during testing when a board already has an ID higher than next_available_ID
    0.6.3   Adding code to enable console commands from the Master
    0.7.0   Implementing "Manual" and "Program" modes
    0.7.1   In preparation for 0.8.0 this revision is designed to
            allow for simplified command line inputs. These inputs 
            will be precursors to the gamepad inputs intended for implementation
            in 0.8.0. 
    0.7.2   The program now sends the ID of the current AUTO tile to the MC. This
            revision will attempt to implement the same functionality for the 
            CURSOR state.
    0.7.3   This revision makes slight changes to the LED blinking patterns in order to
            better indicate the state of operation. Use of Timer1 
            (from USBHID_Integration_Testing) will enable consistent blinking.
            This, however precludes the ability to use the PCM library for sound.
            Sound output will now come from the Master Controller.
    0.7.4   The goal of this revision is to implement PROGRAM mode.
*/

#include<SoftwareSerial.h>

// Pins 2 and 7 are reserved for FTDI comms
// Pins 0 and 1 are reserved for USB comms (during programming only)

#define TRXRATE 19200
#define TERMBAUD 9600
#define LEDPIN 3 //Pin that controls the LEDs
#define SENSORPIN A5 //Pin that reads the pressure sensor input
#define SEEDPIN A1
#define RX_1 4
#define TX_1 5
#define RX_2 6
#define TX_2 8
#define RX_3 9
#define TX_3 10
#define RX_4 12
#define TX_4 13
#define MAX_BUFFER_SIZE 64
#define TIME_OUT 25
#define SENSORTHRESHOLD 200
#define PLAYTIME 3000
#define LOCKTIME 1000
#define NO_RESPONSE 160

//STATE CODES:
#define LISTENING 0
#define CHANNEL_SET 1
#define PROCESS_MSG 2

//COMMAND CODES:
#define COM 170
#define ROGER 171
#define NEGATIVE 172
#define SETID 173
#define SENDID 174
#define SETNEIGHBORS 175
#define IDSET 176
#define NEIGHBORSSET 177
#define STATE_CHANGE 178
#define CURSOR 179
#define AMCURSOR 180
#define SEND_CURSOR 181
#define STEP_ON_ME 182
#define PROG_CURSOR 183
#define AMPCURSOR 184
#define AMAUTO 185
#define IDSENT 186
#define PROG_ME 191
#define UNPROG_ME 192
#define AUTO 200
#define MANUAL 201
#define PROGRAM 202
#define NOCOM 255

SoftwareSerial mySerial[4] = {SoftwareSerial(RX_1,TX_1),SoftwareSerial(RX_2,TX_2),SoftwareSerial(RX_3,TX_3),SoftwareSerial(RX_4,TX_4)};

int active_channel;
int rec_channel;
int rand_channel;
int listen_count;
int state;
uint8_t read_buf[MAX_BUFFER_SIZE];
uint8_t write_buf[MAX_BUFFER_SIZE];
int read_buf_len, write_buf_len;
long waitTime;
uint8_t myID;
int next_available_ID;
long prevTime;
long startTime;
int ledState;
int port_map[4];
int return_path[16];
int return_path_len;
int next_list[4];
int next_list_len;
//int sensorThreshold;
int sensor_flag;
int auto_flag;
int cursor_flag;
int pcursor_flag;
int step_on_me_flag;
int prog_me_flag;
int auto_setup;
long start_lock;
uint8_t randNumber;
long playback_started;
int waitForPlayback = 0;
boolean isBlinking;


/*This function is depricated as of 4/2/15, and will
soon be phased out. Use the functions turn_blink_on() 
and turn_blink_off() for consistent cursor-like blinks.*/
void blink_led(int ledPin){
  long blinkTime = 250;
  long curTime = millis();
  if((curTime - prevTime) > blinkTime){
    prevTime = curTime;
    if(ledState == LOW){
      ledState = HIGH;
    }else{
      ledState = LOW;
    }
  }
  digitalWrite(ledPin, ledState);
}

//Interrupt Service Routine for handling Timer1 interrupts
ISR(TIMER1_COMPA_vect){
  digitalWrite(LEDPIN, !digitalRead(LEDPIN));
  ledState = !ledState;
}
/* Using Timer1 (16-bit counter register) in CTC mode to control
the blinking of tile LEDs.*/
void initialize_timer1(){
  cli();
  TCCR1A = 0;
  TCCR1B = 0;
  OCR1A = 7812;
  turn_blink_off();
  sei();
}

void turn_blink_on(){
  TCCR1B |= (1 << WGM12);
  TCCR1B |= (1 << CS10);
  TCCR1B |= (1 << CS12);
  TIMSK1 |= (1 << OCIE1A);
  isBlinking = true;
}

void turn_blink_off(){
  TCCR1B &= (0 << WGM12);
  TCCR1B &= (0 << CS10);
  TCCR1B &= (0 << CS12);
  TIMSK1 &= (0 << OCIE1A);
  isBlinking = false;
}

int isvalueinarray(int val, int *arr, int size){
    int i;
    for (i=0; i < size; i++) {
        if (arr[i] == val)
            return i;
    }
    return -1;
}

//Write message buffer to serial TX
void writebuffertoserial(int channel, uint8_t *arr, int buf_size) {
  int i;
  int temp_len = buf_size;
  uint8_t temp_buf[buf_size+1];
  const uint8_t start_sig = COM;
  while(temp_len > 0){
    temp_buf[temp_len] = arr[temp_len-1];
    temp_len--;
  }
  temp_buf[0] = start_sig;
  Serial.print("Writing");
  for(i=0;i<buf_size+1;i++){
    Serial.print(", ");
    Serial.print(temp_buf[i], DEC);
  }
  Serial.print(" to serial channel ");
  Serial.println(channel, DEC);
  if(channel > 0 && channel < 5){
    mySerial[channel-1].write(temp_buf, buf_size+1);
  }else{
    Serial.println("Invalid channel selection for writebuffertoserial()");
    Serial.print("Error! cannot write to serial channel ");
    Serial.println(channel);
  }
}

void step_on_me_routine(){
  // indicate that it is waiting to be stepped on and wait for sensor input
    turn_blink_off();
    digitalWrite(LEDPIN, HIGH);
    ledState = HIGH; 
    if(analogRead(SENSORPIN) < SENSORTHRESHOLD){
      //tile has been stepped on
      sensor_flag = 1;
      //startPlayback(happy_data, sizeof(happy_data));
      playback_started = millis();
    }
    if(sensor_flag == 1){
      blink_led(LEDPIN);
      if(millis() - playback_started > PLAYTIME){
        sensor_flag = 0;
        step_on_me_flag = 0;
        //cursor_flag = 1;
      }
    }
}

void auto_routine(){
  Serial.println("Autonomous mode active!");
  //when this signal is received this board should:
  // 1. indicate that it is waiting to be stepped on
  // 2. select the next tile from it's available neighbors, excluding the neighbor from which the signal was first received
  // 3. when stepped on:
  //   A. Indicate that it senses the child by playing the sound file and flashing lights
  //   B. Send the AUTO signal on to the next board
  //   C. Return to the LISTENING state
  if(sensor_flag == 1){
    blink_led(LEDPIN);
    if(millis() - playback_started > PLAYTIME){
      //send the AUTO message to the next board
      //but only if a signal was not just received
      if(state != CHANNEL_SET){
        if(next_list_len == 0){
          setChannel(rec_channel);
        }else{
          setChannel(rand_channel);
        }
        write_buf[0] = AUTO;
        write_buf_len = 1;
        int send_status = send_msg();
        if(send_status != 0){
          Serial.print("Error: status code = ");
          Serial.print(send_status);
          Serial.println(". Something went wrong with board-to-board comms!!!");
        }else{
          state = LISTENING;
        }
      }
      sensor_flag = 0;
      auto_flag = 0;
      next_list_len = 0;
      auto_setup = 0;
      digitalWrite(LEDPIN, LOW);
      ledState = LOW;
      //state = LISTENING;
    } 
  }else{
    //select the next tile
    //create an array of potential next tiles indicated by the respective port number or channel
    if(auto_setup == 0){
      sensor_flag = 0;
      for(int i=0;i<4;i++){
        if((i+1 != rec_channel) && (port_map[i] < 255) && (port_map[i] > 0)){
          next_list[next_list_len] = i+1;
          next_list_len++;
        }
      }
      //if this tile is a dead-end, the result of setChannel will be to not change the active channel.
      //this means the next tile will be the tile from which the AUTO signal was originally received
      //this is the correct behavior, even though it was not explicitly programmed
      Serial.print("received channel: ");
      Serial.println(rec_channel);
      Serial.print("next_list_len: ");
      Serial.println(next_list_len);
      rand_channel = next_list[random(next_list_len)];
      Serial.print("Random channel: ");
      Serial.println(rand_channel);
      auto_setup = 1;
    }
    // indicate that it is waiting to be stepped on and wait for sensor input
    digitalWrite(LEDPIN, HIGH);
    ledState = HIGH; 
    if(analogRead(SENSORPIN) < SENSORTHRESHOLD){
      //tile has been stepped on
      sensor_flag = 1;
      //startPlayback(happy_data, sizeof(happy_data));
      playback_started = millis();
    }
  }
}

int setChannel(int channel){
    if(channel > 0 && channel < 5){
        if(channel == active_channel){
          if(channel > 1){
            mySerial[channel-2].listen();
          }else{
            mySerial[channel].listen();
          }
        }
        mySerial[channel-1].listen();
        delay(100);
        active_channel = channel;
        return 0;
    }else{
        return -1;
    }
}

void set_ID(){
  int send_status = -1;
  /*for(int i=0;i<read_buf_len;i++){
    Serial.print(read_buf[i]);
  }
  Serial.println("");*/
  myID = read_buf[1];
  write_buf[0] = IDSET;
  write_buf[1] = myID;
  write_buf_len = 2;
  send_status = send_msg();
  if(send_status != 0){
    Serial.print("Error: status code = ");
    Serial.print(send_status);
    Serial.println(". Something went wrong with board-to-board comms!!!");
  }else{
    state = LISTENING;
  }
}

void set_neighbors(){
  int send_status = -1;
  return_path_len = 0;
  next_available_ID = read_buf[1];
  for(int j=2;j<read_buf_len;j++){
    return_path[j-2] = read_buf[j];
    Serial.println(return_path[return_path_len]);
    return_path_len++;
  }
  for(int i=0;i<4;i++){
    setChannel(i+1);
    port_map[i] = get_ID();
    if(port_map[i] == NO_RESPONSE){
      port_map[i] = -1;
    }else if(port_map[i] == 255 || port_map[i] >= next_available_ID){  // next_available_ID){ This should fix the issue of having previously assigned IDs during testing
      port_map[i] = send_set_ID();
      if(port_map[i] >= next_available_ID){
        next_available_ID = port_map[i] + 1;
      }
    }
  }
  //find port with ID '0'
  int port_num = 0; //invalid port number
  for(int i=0;i<4;i++){
    if(port_map[i] == return_path[0]){
      port_num = i+1;
    }
  }
  if(port_num == 0){
    Serial.println("no valid port found!");
  }else{
    setChannel(port_num);
  }
  write_buf_len = 0;
  for(int i=1;i<return_path_len;i++){
    write_buf[write_buf_len] = return_path[i];
    write_buf_len++;
  }
  write_buf[write_buf_len] = NEIGHBORSSET;
  write_buf_len++;
  write_buf[write_buf_len] = myID;
  write_buf_len++;
  write_buf[write_buf_len] = port_map[0];
  write_buf_len++;
  write_buf[write_buf_len] = port_map[1];
  write_buf_len++;
  write_buf[write_buf_len] = port_map[2];
  write_buf_len++;
  write_buf[write_buf_len] = port_map[3];
  write_buf_len++;
  write_buf[write_buf_len] = next_available_ID;
  write_buf_len++;
  
  send_status = send_msg();
  if(send_status != 0){
    Serial.print("Error: status code = ");
    Serial.print(send_status);
    Serial.println(". Something went wrong with board-to-board comms!!!");
  }else{
    state = LISTENING;
  }
}

int send_set_ID(){
  int send_status = -1;
  write_buf[0] = SETID;
  write_buf[1] = next_available_ID;
  write_buf_len = 2;
  send_status = send_msg();
  if(send_status != 0){
    //something went wrong with board to board comms
    Serial.print("Error: status code = ");
    Serial.print(send_status);
    Serial.println(". Something went wrong with board-to-board comms!!!");
    return -1;
  }else{
    //wait for confirmation
    int recvd = 0;
    while(recvd == 0){
      delay(1);
      if(mySerial[active_channel-1].read() == COM){
        Serial.println("COM");
        //uint8_t cur_read = -1;
        int cnt = 0;
        while(mySerial[active_channel-1].available() > 0 && cnt < MAX_BUFFER_SIZE && mySerial[active_channel-1].peek() != COM){
          read_buf[cnt] = mySerial[active_channel-1].read();
          cnt++;
          Serial.println("available");
          delay(1);
        }
        read_buf_len = cnt;
      }    
      if(read_buf_len > 0 && read_buf[0] == IDSET){
        send_msg_received();
        recvd = 1;
      }
    }
    return read_buf[1];
  }
}

int get_ID(){
  int send_status = -1;
  write_buf[0] = SENDID;
  write_buf_len = 1;
  send_status = send_msg();
  if(send_status != 0){
  //something went wrong with board to board comms
    Serial.print("Error: status code = ");
    Serial.print(send_status);
    Serial.println(". Something went wrong with board-to-board comms!!!");
    return NO_RESPONSE;
  }else{
    //wait for response
    int recvd = 0;
    while(recvd == 0){
      delay(1);
      Serial.println(mySerial[active_channel-1].peek());
      if(mySerial[active_channel-1].read() == COM){
        Serial.println("COM");
        //uint8_t cur_read = -1;
        int cnt = 0;
        while(mySerial[active_channel-1].available() > 0 && cnt < MAX_BUFFER_SIZE && mySerial[active_channel-1].peek() != COM){
          read_buf[cnt] = mySerial[active_channel-1].read();
          cnt++;
          Serial.println("available");
          delay(1);
        }
        read_buf_len = cnt;
      }    
      if(read_buf_len > 0 && read_buf[0] == IDSENT){
        send_msg_received();
        recvd = 1;
      }
    }
    return read_buf[1];
  }
}

void send_ID(){
    int send_status = -1;
    write_buf[0] = IDSENT;
    write_buf[1] = myID;
    write_buf_len = 2;
    send_status = send_msg();
    if(send_status != 0){
        //something went wrong with board to board comms
        Serial.print("Error: status code = ");
        Serial.print(send_status);
        Serial.println(". Something went wrong with board-to-board comms!!!");
    }else{
      state = LISTENING;
    }
    //writebuffertoserial(active_channel, write_buf, write_buf_len);
}

void prep_msg_to_MC(){
  int port_num = 0; //invalid port number
  for(int i=0;i<4;i++){
    if(port_map[i] == return_path[0]){
      port_num = i+1;
    }
  }
  if(port_num == 0){
    Serial.println("no valid port found!");
  }else{
    setChannel(port_num);
  }
  write_buf_len = 0;
  for(int i=1;i<return_path_len;i++){
    write_buf[write_buf_len] = return_path[i];
    write_buf_len++;
  }
}

int send_msg(){
  int count = 0;
  int confirm_rcvd = 0;
  int temp_buf_len = 0;
  uint8_t temp_buf[MAX_BUFFER_SIZE];
  while(mySerial[active_channel-1].available() > 0)mySerial[active_channel-1].read();
  while(confirm_rcvd != 1 && count < TIME_OUT){
    writebuffertoserial(active_channel, write_buf, write_buf_len);
    delay(30);
    if(mySerial[active_channel-1].read() == COM){
        int cnt = 0;
        while(mySerial[active_channel-1].available() > 0 && mySerial[active_channel-1].peek() != COM){
            temp_buf[cnt] = mySerial[active_channel-1].read();
            cnt++;
        }
        temp_buf_len = cnt;
        if(cnt > 0 && temp_buf[cnt-1] == ROGER){
          confirm_rcvd = 1;
          Serial.println("...");
        }else{
          while(mySerial[active_channel-1].available() > 0)mySerial[active_channel].read();
        }
    }else{
        //if the input buffer doesn't start with '170', flush the garbage.
        while(mySerial[active_channel-1].available() > 0)mySerial[active_channel-1].read();
    }
    count++;
  }
  delay(60);
  while(mySerial[active_channel-1].available() > 0)mySerial[active_channel-1].read();
  read_buf_len = 0;
  
  if(count > TIME_OUT || temp_buf_len <= 0){
    return -1;
  }else{
    return 0;
  }
}

void send_msg_received(){
    uint8_t msg_rcvd = ROGER;
    for(int i=0;i<2;i++){
        writebuffertoserial(active_channel, &msg_rcvd, 1);
        delayMicroseconds(500);
    }
    //returning with the assumption that our confirmation was received
}

void setup(){
  Serial.begin(TERMBAUD);
  Serial.println("attempting to initialize Serial comms...");
  Serial.println("...");
  pinMode(RX_1, INPUT_PULLUP);
  pinMode(TX_1, OUTPUT);
  pinMode(RX_2, INPUT_PULLUP);
  pinMode(TX_2, OUTPUT);
  pinMode(RX_3, INPUT_PULLUP);
  pinMode(TX_3, OUTPUT);
  pinMode(RX_4, INPUT_PULLUP);
  pinMode(TX_4, OUTPUT);
  pinMode(SENSORPIN, INPUT);
  //pinMode(SPKRPIN, OUTPUT);
  for(int i=3;i>=0;i--){
    mySerial[i].begin(TRXRATE);
    Serial.println("...");
  }
  pinMode(LEDPIN, OUTPUT);
  Serial.println("...initialization complete.");
  randomSeed(analogRead(SEEDPIN));
  sensor_flag = 0;
  auto_flag = 0;
  cursor_flag = 0;
  pcursor_flag = 0;
  step_on_me_flag = 0;
  prog_me_flag = 0;
  myID = -1;
  active_channel = 1;
  rec_channel = 1;
  rand_channel = -1;
  listen_count = 1;
  next_available_ID = -1;
  next_list_len =0;
  //sensorThreshold = 200;
  waitTime = 20;
  read_buf_len = write_buf_len = 0;
  for(int i=0;i<4;i++){
    port_map[i] = -1;
  }
  for(int i=0;i<16;i++){
    return_path[i] = -1;
  }
  return_path_len = 0;
  state = LISTENING;
  digitalWrite(LEDPIN, LOW);
  initialize_timer1();
  ledState = LOW;
  prevTime = millis();
}

void loop(){
  switch(state){
    case LISTENING:
        //in listening mode, cycle through all ports until info is received
        setChannel(listen_count);
        Serial.println(active_channel,DEC);
        startTime = millis();
        while(mySerial[active_channel-1].available() == 0 && ((millis() - startTime) < waitTime)){}
        if(mySerial[active_channel-1].available() > 0){
            state = CHANNEL_SET;
            start_lock = millis();
            Serial.println("mode: CHANNEL_SET");
        }else{
            listen_count++;
            delay(3);
        }
        if(listen_count > 4){
            listen_count = 1;
        }
        if(step_on_me_flag == 1){
            step_on_me_routine();
        }else if((cursor_flag == 1) && (!isBlinking)){
          //cursor blink 500ms on 500ms off
          turn_blink_on(); //digitalWrite(LEDPIN, HIGH);
        }else if((cursor_flag == 0) && (isBlinking)){
          turn_blink_off();
        }
        if(prog_me_flag == 1){
          digitalWrite(LEDPIN, HIGH);
          ledState = HIGH;
        }else if((pcursor_flag == 1) && (!isBlinking)){
          turn_blink_on();
        }else if((pcursor_flag == 0) && (isBlinking)){
          turn_blink_off();
        }
        if(auto_flag == 1){
            auto_routine();
        }
        break;
    case CHANNEL_SET:
        Serial.println(active_channel);
        Serial.println(mySerial[active_channel-1].peek(),DEC);
        //delay(1);  // will need this delay when the Serial.println()'s are removed. As they are acting as the needed delay.
        if(mySerial[active_channel-1].read() == COM){
            int cnt=0;
            while(mySerial[active_channel-1].available() > 0 && cnt < MAX_BUFFER_SIZE && (uint8_t)mySerial[active_channel-1].peek() != COM){
                read_buf[cnt] = mySerial[active_channel-1].read();
                cnt++;
                delay(1);
            }
            read_buf_len = cnt;
            send_msg_received();
            state = PROCESS_MSG;
            Serial.println("mode: PROCESS_MSG");
        }else{
            while(mySerial[active_channel-1].available() > 0)mySerial[active_channel-1].read();
        }
        //if a valid signal isn't received before LOCKTIME is up, the program should return to listening
        if(state == CHANNEL_SET && millis() - start_lock > LOCKTIME){
          Serial.println("Lock time exceeded without valid data!!");
          state = LISTENING;
        }
        break;
    case PROCESS_MSG:
        //once the program reaches this point, it has a message
        //consisting of uint8_t values in the read_buf array
        //according to the COMMAND CODE scheme outlined above
        //if the first value in the read_buf array is greater than
        //170, then the message has reached its destination and
        //this COMMAND CODE should be executed
        Serial.print("message received: ");
        for(int i=0;i<read_buf_len;i++){
          Serial.print(read_buf[i]);
          Serial.print(", ");
        }
        Serial.println("");
        if(read_buf[0] > ROGER){
          int port = 0;
          int send_status = -1;
            //execute COMMAND CODE
            switch(read_buf[0]){
                case SENDID:
                    send_ID();
                    break;
                case SETID:
                    set_ID();
                    break;
                case SETNEIGHBORS:
                    set_neighbors();
                    break;
                case AUTO:
                    auto_flag = 1;
                    auto_setup = 0;
                    rec_channel = active_channel;
                    prep_msg_to_MC();
                    write_buf[write_buf_len] = AMAUTO;
                    write_buf_len++;
                    write_buf[write_buf_len] = myID;
                    write_buf_len++;
                    send_msg();
                    state = LISTENING;
                    break;
                case CURSOR:
                    step_on_me_flag = 0;
                    //set cursor_flag
                    cursor_flag = 1;
                    prep_msg_to_MC();
                    write_buf[write_buf_len] = AMCURSOR;
                    write_buf_len++;
                    write_buf[write_buf_len] = myID;
                    write_buf_len++;
                    send_msg();
                    
                    state = LISTENING;
                    //send "i_am_cursor" back to MC
                    break;
                case PROG_CURSOR:
                    step_on_me_flag = 0;
                    //set cursor_flag
                    pcursor_flag = 1;
                    prep_msg_to_MC();
                    write_buf[write_buf_len] = AMPCURSOR;
                    write_buf_len++;
                    write_buf[write_buf_len] = myID;
                    write_buf_len++;
                    send_msg();
                    
                    state = LISTENING;
                    //send "i_am_cursor" back to MC
                    break;
                case SEND_CURSOR:
                    //read_buf should look like: [SEND_CURSOR,to-tile-ID]
                    //next_tile = read_buf[1];
                    //int send_status = -1;
                    port = isvalueinarray(read_buf[1], port_map, 4) + 1;
                    setChannel(port);
                    write_buf[0] = CURSOR;
                    write_buf_len = 1;
                    send_status = send_msg();
                    if(send_status != 0){
                      //something went wrong with board to board comms
                      Serial.print("Error: status code = ");
                      Serial.print(send_status);
                      Serial.println(". Something went wrong with board-to-board comms!!!");
                    }else{
                      turn_blink_off();
                      digitalWrite(LEDPIN, LOW);
                      ledState = LOW;
                      cursor_flag = 0;
                      state = LISTENING;
                    }
                    //state = LISTENING;
                    break;
                case STEP_ON_ME:
                    step_on_me_flag = 1;
                    turn_blink_off();
                    state = LISTENING;
                    break;
                case PROG_ME:
                    prog_me_flag = 1;
                    turn_blink_off();
                    state = LISTENING;
                    break;
                case UNPROG_ME:
                    prog_me_flag = 0;
                    state = LISTENING;
                    break;
                case STATE_CHANGE:
                    state = read_buf[1];
                    auto_flag = 0;
                    cursor_flag = 0;
                    pcursor_flag = 0;
                    turn_blink_off();
                    step_on_me_flag = 0;
                    //prog_me_flag = 0;
                    digitalWrite(LEDPIN, LOW);
                    ledState = LOW;
                    break;
                default:
                    state = LISTENING;
                    break;
            }
        }else{
            //pass the message onto the next board
            int send_status = -1;
            int port = isvalueinarray(read_buf[0], port_map, 4) + 1;
            setChannel(port);
            for(int i=0;i<read_buf_len;i++){
              write_buf[i] = read_buf[i+1];
              write_buf_len = read_buf_len-1;
            }
            send_status = send_msg();
            if(send_status != 0){
              //something went wrong with board to board comms
              Serial.print("Error: status code = ");
              Serial.print(send_status);
              Serial.println(". Something went wrong with board-to-board comms!!!");
            }else{
              state = LISTENING;
            }
        }
        break;
    default:
      break;
  }
}
